package cs470.kalman;

import cs470.kalman.exception.KalmanException;
import Code.Point;

public class Matrix 
{
	double[][] matrix;
	
	/**********************************************
	 * Constructs a matrix object
	 * @param newMatrix the matrix being manipulated
	 * @throws Exception 
	 **********************************************/
	public Matrix(double[][] newMatrix)
	{
		try
		{
			int cols = newMatrix[0].length;
			for (int i = 0; i < newMatrix.length; i++)
			{
				if (cols != newMatrix[i].length)
					throw new KalmanException("Invalid matrix");
			}
			matrix = newMatrix;
		}
		catch(KalmanException e)
		{
			System.out.println(e.getMessage());
		}
	}
	
	/***********************************************
	 * Adds another matrix to this matrix
	 * @param other the other matrix
	 * @return the resulting matrix
	 * @throws Exception 
	 ***********************************************/
	public Matrix add(Matrix other) throws KalmanException
	{
		int rows = matrix.length;
		int cols = matrix[0].length;
		if (rows != other.matrix.length || cols != other.matrix[0].length)
			throw new KalmanException ("Invalid dimensions");
		double[][] tempMatrix = new double[rows][cols];
		
		for (int i = 0; i < rows; i++)
		{
			for (int j = 0; j < cols; j++)
			{
				tempMatrix[i][j] = matrix[i][j] + other.matrix[i][j];
			}
		}
		
		return new Matrix(tempMatrix);
	}
	
	public Matrix subtract(Matrix other) throws KalmanException
	{
		int rows = matrix.length;
		int cols = matrix[0].length;
		if (rows != other.matrix.length || cols != other.matrix[0].length)
			throw new KalmanException ("Invalid dimensions");
		double[][] tempMatrix = new double[rows][cols];
		
		for (int i = 0; i < rows; i++)
		{
			for (int j = 0; j < cols; j++)
			{
				tempMatrix[i][j] = matrix[i][j] - other.matrix[i][j];
			}
		}
		
		return new Matrix(tempMatrix);
	}
	
	/****************************************
	 * Transposes the current matrix
	 * @return the transposed matrix
	 * @throws Exception 
	 */
	public Matrix transpose() throws KalmanException
	{
		int row = matrix.length;
		int col = matrix[0].length;
		double[][] tempMatrix = new double[col][row];
		
		for (int i = 0; i < row; i++)
		{
			for (int j = 0; j < col; j++)
			{
				tempMatrix[j][i] = matrix[i][j];
			}
		}
		
		return new Matrix(tempMatrix);
	}
	
	/*************************************************
	 * Multiply the current matrix to another matrix
	 * @param other the matrix to multiply by
	 * 		this x other
	 * @return the product matrix
	 * @throws Exception 
	 */
	public Matrix multiply(Matrix other) throws KalmanException
	{
		int rowsThis = matrix.length;
		int colsThis = matrix[0].length;
		int rowsOther = other.matrix.length;
		int colsOther = other.matrix[0].length;
		if (colsThis != rowsOther)
			throw new KalmanException("Invalid dimensions");
		double[][] tempMatrix = new double[rowsThis][colsOther];
		
		for (int i = 0; i < rowsThis; i++)
		{
			for (int j = 0; j < colsOther; j++)
			{
				tempMatrix[i][j] = 0;
				for (int k = 0; k < colsThis; k++)
				{
					tempMatrix[i][j] += matrix[i][k] * other.matrix[k][j];
				}
			}
		}
		
		return new Matrix(tempMatrix);
	}
	
	@Override
	public boolean equals(Object obj)
	{
		Matrix o = (Matrix)obj;
		if (o.matrix.length != matrix.length)
			return false;
		if (o.matrix[0].length != matrix[0].length)
			return false;
		
		for (int row = 0; row < matrix.length; row++)
		{
			for (int col = 0; col < matrix[0].length; col++)
			{
				double diff = Math.abs(matrix[row][col] - o.matrix[row][col]);
				if (Double.compare(0.000000001, diff)  <=0)
				{
					return false;
				}
			}
		}
		return true;
	}
	
	@Override
	public String toString()
	{
		StringBuilder sb = new StringBuilder();
		for (int i = 0; i < matrix.length; i++)
		{
			for (int j = 0; j < matrix[0].length; j++)
			{
				System.out.print(matrix[i][j] + "\t");
			}
			System.out.println();
		}
		
		return sb.toString();
	}
	
	//Inverts the current matrix.
	//NOTE: returns null if the matrix is not invertable
	public Matrix invert() throws KalmanException
	{
		if (matrix.length != matrix[0].length)
			return null;
		//generate the matrix to start
		double[][] temp = new double[matrix.length][matrix.length*2];
		for (int i = 0; i < matrix.length; i++)
		{
			for (int j = 0; j < 2*matrix.length; j++)
			{
				if (j < matrix.length)
					temp[i][j] = matrix[i][j];
				else
				{
					if (i == j-matrix.length)
						temp[i][j] = 1;
					else
						temp[i][j] = 0;
				}
			}
		}
		
		//invert
		for (int i = 0; i < matrix.length; i++)
		{
			double divisor = temp[i][i];
			if (divisor == 0)
			{
				double tempNum = 0;
				for (int row = i+1; row < matrix.length; row++)
				{
					if (temp[row][i] != 0)
					{
						for (int col = 0; col < matrix.length *2; col++)
						{
							tempNum = temp[i][col];
							temp[i][col] = temp[row][col];
							temp[row][col] = tempNum;
							divisor = temp[i][i];
						}
					}
				}
			}
			
			if (divisor == 0)
				return null;
			//divide current row
			for (int cols = i; cols < matrix.length*2; cols++)
			{
				temp[i][cols] /= divisor;
			}
			
			//0 out others rows
			for (int row = 0; row < matrix.length; row++)
			{
				if (row != i)
				{
					double divisor2 = temp[row][i];
					if (divisor2 != 0)
					{
						for (int cols = i; cols < matrix.length*2; cols++)
						{
							temp[row][cols] = temp[row][cols] - temp[i][cols]*divisor2;
						}
					}
				}
			}
		}
		
		double[][] tempMatrix = new double[matrix.length][matrix.length];
		for (int row = 0; row < matrix.length; row++)
		{
			for (int col = 0; col < matrix.length; col++)
			{
				tempMatrix[row][col] = temp[row][col+matrix.length];
			}
		}
		return new Matrix(tempMatrix);
	}
	
	public double getValue(int row, int col)
	{
		return matrix[row][col];
	}
	
	public static Matrix getMatrixFromPoint(final Point point) {
		double[][] array = new double[][]{{point.getX()},{ point.getY()}};
			return new Matrix(array);
	}

}
